﻿/*
 * lane_kalman.cpp
 *
 *  Created on: May 9, 2018
 *      Author: geoff
 * *
 */

#include "lane_kalman.h"
#include "calculate_tools.h"

namespace rmd {

// Define the system's own error when defining the system state transition
// The better parameters for the straight line: define CKalman::kX1Cov 10
// CKalman::kX1vCov 5 CKalman::kX2Cov
// 30  CKalman::kX2vCov 3
// Reduce the speed error to get a smoother result
const int CKalman::kX1Cov = 6;   // 10;
const int CKalman::kX1vCov = 2;  // 3;
const int CKalman::kX2Cov = 10;  // 20;
const int CKalman::kX2vCov = 2;  // 3;
const int CKalman::kX3Cov = 10;  // 30;
const int CKalman::kX3vCov = 2;  // 3;

// Defining the errors caused by observations
const int CKalman::kMx1Cov = 2;  // 4;
const int CKalman::kMx2Cov = 3;  // 6;
const int CKalman::kMx3Cov = 4;  // 8;
const int CKalman::kMx4Cov = 4;  // 8;

// Define error coefficients initialized using observations
const int CKalman::kX1Covk = 1;
const int CKalman::kX1vCovk = 6;  // 8
const int CKalman::kX2Covk = 1;
const int CKalman::kX2vCovk = 6;  // 8
const int CKalman::kX3Covk = 1;
const int CKalman::kX3vCovk = 6;  // 8

// The number of consecutive observations after reaching continuous observation
// conditions is considered as stable
const int CKalman::kStableTime = 2;

// Thresholds for jumping are considered
const int CKalman::kJumpThread = 100;

// Think of a complete trace failure threshold
const int CKalman::kErrorThread = 200;

// Aggregate bases that are considered good
const int CKalman::kQualityTime = 2;

const int CKalman::kLinLooseMaxTime = 7;

const int CKalman::kContinuGood = 100;

CKalman::CKalman() {}

CKalman::~CKalman() {}

CKalman* CKalman::instance_ = NULL;
CKalman* CKalman::Instance() {
  if (instance_ == NULL) {
    instance_ = new CKalman();
    instance_->EkfInitialization();
  }
  return instance_;
}

std::vector<LaneLine> CKalman::Update(DataFrame* data_frame,
                                      std::vector<LaneLine>& now_lines) {
  LaneLine temp_line;
  std::vector<LaneLine> filter_lines;

  lane_width = data_frame->refine_lane_width;

  for (int i = 0; i < now_lines.size(); i++) {
    EkfProcessSingleLine(now_lines[i]);
  }

  for (int i = 0; i < kNumEkf; i++) {
    //本次观测不到但滤波器仍然有效的
    //   当前都观测不到 并且 工作状态表示还在工作
    if (line_ekf_[i].point_middle.flag_work == true &&
        line_ekf_[i].point_middle.flag_obsev == false) {
      // if ((line_bottom_[i].flag_obsev==false))
      CMatrix z_measure1, z_measure2, z_measure3;
      z_measure1.Init(1, 1);
      z_measure1.SetElement(0, 0, 0);  // line_ekf_[i].Line.Line_pre[1].x);
      z_measure2.Init(1, 1);
      z_measure2.SetElement(0, 0, 0);  // line_ekf_[i].Line.Line_pre[0].x);
      z_measure3.Init(1, 1);
      z_measure3.SetElement(0, 0, 0);  // line_ekf_[i].Line.Line_pre[2].x);

      //指定状态转移误差
      SetProcessNoiseCov(line_ekf_[i].point_bottom, kX1Cov, kX1vCov);
      SetProcessNoiseCov(line_ekf_[i].point_middle, kX2Cov, kX2vCov);
      SetProcessNoiseCov(line_ekf_[i].point_top, kX3Cov, kX3vCov);

      //指定观测误差
      SetObservationNoiseCov(line_ekf_[i].point_bottom, kMx1Cov);
      SetObservationNoiseCov(line_ekf_[i].point_middle, kMx2Cov);
      SetObservationNoiseCov(line_ekf_[i].point_top, kMx3Cov);

      //因为此时观测为零，所以滤波执行的是预测
      EkfMainProcess(line_ekf_[i].point_bottom, z_measure1, kX1Covk, kX1vCovk);
      EkfMainProcess(line_ekf_[i].point_middle, z_measure2, kX2Covk, kX2vCovk);
      EkfMainProcess(line_ekf_[i].point_top, z_measure3, kX3Covk, kX3vCovk);

      //对角度值的状态寄存器进行状态更新。
      line_ekf_[i].Midle_Near_Angle_Cannotseetime++;
      if (line_ekf_[i].Midle_Near_Angle_Cannotseetime >= kLinLooseMaxTime)
        line_ekf_[i].Midle_Near_Angle_Flagwork = false;

      GetEkfLineResult(line_ekf_[i]);  //将值保存起来
    }
  }
  //向外传值
  // cout<<"thread is : GOODTIME="<<GOODTIME<<"BADTIME="<<BADTIME<<"
  // CANCELTIME="<<CANCELTIME<<endl;
  for (int i = 0; i < kNumEkf; i++) {
    if (line_ekf_[i].point_middle.flag_work == true) {
      //将变化比较剧烈的滤波器flag重置
      if (line_ekf_[i].point_middle.flag_is_good == true) {
        int near_middle_last_pos =
            line_ekf_[i].point_middle.LastState.GetElement(0, 0);
        int near_middle_now_pos =
            line_ekf_[i].point_middle.NowState.GetElement(0, 0);
        //属于跟踪错误的情况
        if (abs(near_middle_last_pos - near_middle_now_pos) >= kErrorThread) {
          line_ekf_[i].point_middle.flag_is_good = false;
          line_ekf_[i].point_middle.time_continu_good = 0;
          line_ekf_[i].point_middle.flag_stable = false;
          line_ekf_[i].point_middle.flag_unconfirm = true;
          line_ekf_[i].point_middle.flag_work = false;  //删除滤波器
          line_ekf_[i].point_bottom.flag_work = false;
          line_ekf_[i].point_top.flag_work = false;
          line_ekf_[i].index = -1;
          // cout<<"ERRORTHREAD happened! "<<endl;
        } else  //属于小跳变
        {
          //属于怀疑的情况
          if (abs(near_middle_last_pos - near_middle_now_pos) >= kJumpThread) {
            line_ekf_[i].point_middle.flag_is_good = false;
            line_ekf_[i].point_middle.time_continu_good = 0;
            line_ekf_[i].point_middle.flag_stable = false;
            line_ekf_[i].point_middle.flag_unconfirm = true;
            // cout<<"JUMPTHREAD happened! "<<endl;
          }
        }
      }

      temp_line = line_ekf_[i].Line;
      //把需要的几个状态重置
      temp_line.nQuality = 0;
      temp_line.bIsTrue = false;

      //补救的方法
      //      if (line_ekf_[i].point_middle.time_continu_obsev <= 1 &&
      //          line_ekf_[i].point_middle.time_continu_good > 2)
      //        line_ekf_[i].point_middle.time_continu_good = 0;

      //提出ekf的几个状态标志 由低到高
      if (line_ekf_[i].point_middle.flag_unconfirm == true)
        temp_line.nQuality = 0;  //不可信
      if (line_ekf_[i].point_middle.flag_obsev == true)
        temp_line.nQuality = 1;  //能够观察到
      if (line_ekf_[i].point_middle.flag_is_good == true)
        temp_line.nQuality = 2;  //已经可以连续观测，可以进行输出
      // if (line_bottom_[i].flag_is_good == true && line_bottom_[i].flag_stable
      // == false) temp_line.nQuality = 3;

      if (line_ekf_[i].point_middle.flag_stable == true)
        //稳定观测的次数
        temp_line.nQuality =
            line_ekf_[i].point_middle.time_continu_good + kQualityTime;

      if (line_ekf_[i].point_middle.flag_obsev == true)
        temp_line.bIsTrue = true;

      filter_lines.push_back(temp_line);
    }

    //保证下次滤波前的状态是重置的
    line_ekf_[i].point_bottom.flag_obsev = false;
    line_ekf_[i].point_middle.flag_obsev = false;
    line_ekf_[i].point_top.flag_obsev = false;
  }

  return filter_lines;
}

void CKalman::EkfInitialization() {
  for (int i = 0; i < kNumEkf; i++) {
    line_ekf_[i].index = -1;
    EkfSingleParaInitialization(line_ekf_[i].point_bottom);
    EkfSingleParaInitialization(line_ekf_[i].point_middle);
    EkfSingleParaInitialization(line_ekf_[i].point_top);

    // line angle init
    line_ekf_[i].Midle_Near_Angle_Lable = 0.0;
    line_ekf_[i].Midle_Near_Angle_Flagwork = false;
    line_ekf_[i].Midle_Near_Angle_Cannotseetime = 0;
  }

  matrix_f_.Init(2, 2);
  matrix_h_.Init(1, 2);

  //对状态矩阵进行赋值
  matrix_f_ = SetValue4Cub(1, 1, 0, 1);  //状态转移矩阵
  matrix_h_.SetElement(0, 0, 1);         //观测矩阵
  matrix_h_.SetElement(0, 1, 0);
}

void CKalman::EkfMainProcess(EKF_PointPara& ekf_point, CMatrix _z_measure,
                             double _X_Cov_K, double _X_V_Cov_K) {
  //判断当前帧有无观测值
  // PrintValue(_z_measure);
  if (_z_measure.GetElement(0, 0) != 0)
    ekf_point.flag_obsev = true;
  else
    ekf_point.flag_obsev = false;
  // cout<<"flag="<<_ekf.flag_obsev<<" "<<_ekf.flag_first<<endl;
  // cout<<

  if (ekf_point.flag_obsev == true)  //如果本次有观测
  {
    SetObserve(ekf_point, _z_measure);  //观测赋值
    ekf_point.time_no_obsev = 0;        //持续不可见计数清零
    ekf_point.flag_work = true;         //滤波器开始工作
    ekf_point.time_continu_obsev++;     //持续观测计数累加
    ekf_point.flag_unconfirm = false;   //不稳定的标志位重置

    //如果达到持续观测_GoodTime次，将可信标志位置为真
    if (ekf_point.time_continu_obsev >= kEkfGoodTime) {
      ekf_point.flag_is_good = true;
    }
    if (ekf_point.flag_is_good == true) {
      ekf_point.time_continu_good++;
      if (ekf_point.time_continu_good >= kContinuGood)
        ekf_point.time_continu_good = kContinuGood;  //防止越界
      if (ekf_point.time_continu_good >= kStableTime) {
        //当足够好的时候，就设为稳定。如果中间有一帧没有观测到，后来又观测到了，因为flag_is_good和time_continu_good没有
        //置位的原因，很快就能够达到true的状态。
        ekf_point.flag_stable = true;
      }
    }

    //表征是否是首次观测，如果是则进入滤波器初始化状态
    if (ekf_point.flag_first == true) {
      ekf_point.flag_first = false;
      Initialize(ekf_point, _X_Cov_K, _X_V_Cov_K);
      EkfPredict(ekf_point);
      ekf_point.flag_unconfirm = false;  //不可信置为false，认为是可信的
      ekf_point.flag_stable = false;  //稳定置为false， 认为是不稳定的

      ekf_point.flag_is_good = false;   //表征滤波器输出值是否可信
      ekf_point.time_continu_good = 0;  //用于计数可用观测的次数

    } else  //如果不是则进入持续滤波状态
    {
      EkfUpdate(ekf_point);
      EkfPredict(ekf_point);
    }
  } else  //如果本次观测不到
  {
    if (ekf_point.flag_work == true) {
      ekf_point.time_no_obsev++;
      ekf_point.time_continu_obsev = 0;

      //如果持续BadTime次观测不到，就认为输出值不可信，没有达到这一条件，下面的值是不变的。
      if (ekf_point.time_no_obsev >= kEkfBadTime) {
        ekf_point.flag_stable = false;
        ekf_point.flag_is_good = false;
        ekf_point.time_continu_good = 0;
        ekf_point.flag_unconfirm = true;
      }

      //如果持续CancelTime次观测不到，将滤波器停止，同时将首次观测置为true，下一次观测来了就重启滤波器。
      if (ekf_point.time_no_obsev >= kEkfCancelTime) {
        ekf_point.flag_work = false;
        ekf_point.flag_first = true;
      }
      EkfPredict(ekf_point);
    }
  }
}

//对滤波结果进行整理，转化为标准的直线数据
void CKalman::GetEkfLineResult(EKF_Para& line_ekf) {
  if (line_ekf.point_middle.flag_first == false)
  //如果不是首次观测，首次观测的则不用计算
  {
    double bottom_y = line_ekf.point_bottom.y_coord;
    double middle_y = line_ekf.point_middle.y_coord;
    double top_y = line_ekf.point_top.y_coord;

    //当前值：
    cv::Point point_up_now =
        cv::Point(line_ekf.point_top.NowState.GetElement(0, 0), top_y);
    cv::Point point_middle_now =
        cv::Point(line_ekf.point_middle.NowState.GetElement(0, 0), middle_y);
    cv::Point point_bottom_now =
        cv::Point(line_ekf.point_bottom.NowState.GetElement(0, 0), bottom_y);

    //预测值：
    cv::Point point_up_pre =
        cv::Point(line_ekf.point_top.PredictState.GetElement(0, 0), top_y);
    cv::Point point_middle_pre = cv::Point(
        line_ekf.point_middle.PredictState.GetElement(0, 0), middle_y);
    cv::Point point_bottom_pre = cv::Point(
        line_ekf.point_bottom.PredictState.GetElement(0, 0), bottom_y);

    //预测值：
    cv::Point point_up_last =
        cv::Point(line_ekf.point_top.LastState.GetElement(0, 0), top_y);
    cv::Point point_middle_last =
        cv::Point(line_ekf.point_middle.LastState.GetElement(0, 0), middle_y);
    cv::Point point_bottom_last =
        cv::Point(line_ekf.point_bottom.LastState.GetElement(0, 0), bottom_y);

    //传值
    //----------当前帧-----------------[2]上端延长端点，[3]下端延长端点
    line_ekf.Line.Line_current[0] = point_middle_last;
    line_ekf.Line.Line_current[1] = point_bottom_last;
    line_ekf.Line.Line_current[2] = point_up_last;
    line_ekf.Line.Line_current[3] = point_middle_last;

    //----------预测帧------------------
    line_ekf.Line.Line_pre[0] = point_middle_pre;
    line_ekf.Line.Line_pre[1] = point_bottom_pre;
    line_ekf.Line.Line_pre[2] = point_up_pre;
    line_ekf.Line.Line_pre[3] = point_middle_pre;

    line_ekf.Line.Line_now[0] = point_middle_now;
    line_ekf.Line.Line_now[1] = point_bottom_now;
    line_ekf.Line.Line_now[2] = point_up_now;
    line_ekf.Line.Line_now[3] = point_middle_now;

    if (line_ekf.point_middle.flag_is_good == false) {
      //当线不稳定的时候取消感兴趣区域
      // ekf_bottom.Line.nDistance[0]=0;
      // ekf_bottom.Line.nDistance[1]=0;
    }
  }
}

//状态转移误差矩阵的赋值
// X_Cov是跟踪参数的方差，X_V_Cov是参数变化地方差
void CKalman::SetProcessNoiseCov(EKF_PointPara& ekf_point, double X_Cov,
                                 double X_V_Cov) {
  ekf_point.G.Zero();
  ekf_point.G.SetElement(0, 0, X_Cov * X_Cov);
  ekf_point.G.SetElement(1, 1, X_V_Cov * X_V_Cov);
}

//观测误差矩阵的赋值
// X_Cov是观测值的方差
void CKalman::SetObservationNoiseCov(EKF_PointPara& ekf_point, double X_Cov) {
  ekf_point.R.Zero();
  ekf_point.R.SetElement(0, 0, X_Cov * X_Cov);
}

void CKalman::EkfProcessSingleLine(LaneLine now_line) {
  bool id_new_add = true;
  int id_index = -1;
  for (int i = 0; i < kNumEkf; i++) {
    //判断滤波器序列中是否有对应的线
    if (now_line.nIndex == line_ekf_[i].index) {
      id_index = i;
      id_new_add = false;
      break;
    }
  }

  if (id_index == -1) {
    for (int i = 0; i < kNumEkf; i++) {
      if (line_ekf_[i].point_middle.flag_work == false) {
        id_index = i;
        break;
      }
    }
    if (id_index == -1) id_index = kNumEkf - 1;
  }

  line_ekf_[id_index].Line = now_line;
  line_ekf_[id_index].index = now_line.nIndex;

  double top_x = 0.0;
  double middle_x = 0.0;
  double bottom_x = 0.0;
  double mid2bot_angle = -kUnknowValue;

  double bottom_y = now_line.near_field[3].y;
  double middle_y = now_line.near_field[2].y;
  if (now_line.nDistance[0] == 0) middle_y = now_line.middle_field[3].y;
  double top_y = now_line.middle_field[2].y;

  //  if (id_new_add)
  {
    line_ekf_[id_index].point_bottom.y_coord = bottom_y;
    line_ekf_[id_index].point_middle.y_coord = middle_y;
    line_ekf_[id_index].point_top.y_coord = top_y;
  }

  bottom_x = now_line.near_field[3].x;
  top_x = now_line.middle_field[2].x;
  middle_x = (now_line.near_field[2].x + now_line.middle_field[3].x) / 2;

  mid2bot_angle = DoubleLineAngle(
      cv::Point2f(top_x, top_y), cv::Point2f(middle_x, middle_y),
      cv::Point2f(middle_x, middle_y), cv::Point2f(bottom_x, bottom_y));

  line_ekf_[id_index].Midle_Near_Angle_Lable = mid2bot_angle;
  line_ekf_[id_index].Midle_Near_Angle_Flagwork = true;
  line_ekf_[id_index].Midle_Near_Angle_Cannotseetime = 0;

  // 指定状态转移误差
  SetProcessNoiseCov(line_ekf_[id_index].point_bottom, CKalman::kX1Cov,
                     CKalman::kX1vCov);
  SetProcessNoiseCov(line_ekf_[id_index].point_middle, CKalman::kX2Cov,
                     CKalman::kX2vCov);
  SetProcessNoiseCov(line_ekf_[id_index].point_top, CKalman::kX3Cov,
                     CKalman::kX3vCov);

  SetObservationNoiseCov(line_ekf_[id_index].point_bottom, CKalman::kMx1Cov);
  SetObservationNoiseCov(line_ekf_[id_index].point_middle, CKalman::kMx2Cov);
  SetObservationNoiseCov(line_ekf_[id_index].point_top, CKalman::kMx3Cov);

  CMatrix z_measure1, z_measure2, z_measure3;
  z_measure1.Init(1, 1);
  z_measure1.SetElement(0, 0, bottom_x);
  z_measure2.Init(1, 1);
  z_measure2.SetElement(0, 0, middle_x);
  z_measure3.Init(1, 1);
  z_measure3.SetElement(0, 0, top_x);

  EkfMainProcess(line_ekf_[id_index].point_bottom, z_measure1, kX1Covk,
                 kX1vCovk);
  EkfMainProcess(line_ekf_[id_index].point_middle, z_measure2, kX2Covk,
                 kX2vCovk);
  EkfMainProcess(line_ekf_[id_index].point_top, z_measure3, kX3Covk, kX3vCovk);

  GetEkfLineResult(line_ekf_[id_index]);  //将融合值保存起来
}

void CKalman::EkfSingleParaInitialization(EKF_PointPara& point_ekf) {
  point_ekf.PredictState.Init(2, 1);
  point_ekf.UpdateState.Init(2, 1);
  point_ekf.NowState.Init(2, 1);

  point_ekf.PredictCovariance.Init(2, 2);
  point_ekf.UpdateCovariance.Init(2, 2);
  point_ekf.NowConvariance.Init(2, 2);

  point_ekf.G.Init(2, 2);
  point_ekf.Z.Init(1, 1);
  point_ekf.Z0.Init(1, 1);
  point_ekf.R.Init(1, 1);

  point_ekf.S.Init(1, 1);
  point_ekf.Gain.Init(2, 1);

  point_ekf.flag_obsev = false;
  point_ekf.flag_first = true;
  point_ekf.flag_work = false;
  point_ekf.flag_is_good = false;

  point_ekf.time_no_obsev = 0;
  point_ekf.time_continu_obsev = 0;
  point_ekf.time_continu_good = 0;
}

//卡尔曼滤波的初始化，使用第一个观测值初始化整个状态。
// X_Cov_K是初始化时跟踪参数状态转移误差的放大系数，X_V_Cov_K是初始化时参数变化状态转移误差的放大系数
void CKalman::Initialize(EKF_PointPara& point_ekf, double X_Cov_K,
                         double X_V_Cov_K) {
  // 对误差矩阵进行赋值
  double x11 = X_Cov_K * point_ekf.R.GetElement(0, 0);
  double x22 = X_V_Cov_K * point_ekf.R.GetElement(0, 0);
  point_ekf.NowConvariance = SetValue4Cub(x11, 0, 0, x22);

  point_ekf.PredictCovariance = point_ekf.NowConvariance;
  point_ekf.UpdateCovariance = point_ekf.NowConvariance;
  // ekf.NowConvariance=ekf.BaseCovariance;

  //对向量进行赋值
  point_ekf.NowState = SetValue2T(point_ekf.Z.GetElement(0, 0), 0);
  point_ekf.PredictState = point_ekf.NowState;
  point_ekf.UpdateState = point_ekf.NowState;
  // ekf.NowState=ekf.BaseState;
  point_ekf.LastState = point_ekf.NowState;  //保存历史帧的数据

  // cout<<"---------------at init stage:"<<endl;
  // cout<<"BaseStae = "<<endl;
  // PrintValue(ekf.NowState);
  // PrintValue(ekf.Z);
  // PrintValue(ekf.G);
}

void CKalman::ObserveFromState(EKF_PointPara& point_ekf) {
  int Row = point_ekf.PredictState.m_nNumRows;
  int Cols = point_ekf.PredictState.m_nNumColumns;
  int Z_Row = Row / 2;
  for (int i = 0; i < Z_Row; i++) {
    point_ekf.Z0.SetElement(i, 0, point_ekf.PredictState.GetElement(i, 0));
  }
}
void CKalman::EkfPredict(
    EKF_PointPara& point_ekf)  // T means the Sample time interval
{
  if (point_ekf.flag_obsev ==
      false)  //在观测为零的情况下，使用上一帧的预测值作为本帧的输出值。
  {
    point_ekf.LastState = point_ekf.NowState;  //保存上一帧的数据
    point_ekf.NowState = point_ekf.PredictState;
    point_ekf.NowConvariance = point_ekf.PredictCovariance;
  }

  point_ekf.PredictState = matrix_f_ * point_ekf.NowState;  //由历史获得状态预测
  point_ekf.PredictCovariance =
      matrix_f_ * point_ekf.NowConvariance * (matrix_f_.Transpose()) +
      point_ekf.G;

  ObserveFromState(point_ekf);
}
//获得融合值
void CKalman::EkfUpdate(EKF_PointPara& point_ekf) {
  point_ekf.LastState = point_ekf.NowState;  //保存上一帧的数据
  point_ekf.S =
      matrix_h_ * point_ekf.PredictCovariance * (matrix_h_.Transpose()) +
      point_ekf.R;
  point_ekf.S.InvertGaussJordan();
  point_ekf.Gain =
      point_ekf.PredictCovariance * (matrix_h_.Transpose()) * point_ekf.S;

  point_ekf.UpdateState =
      point_ekf.PredictState + point_ekf.Gain * (point_ekf.Z - point_ekf.Z0);
  point_ekf.UpdateCovariance =
      point_ekf.PredictCovariance -
      point_ekf.Gain * matrix_h_ * point_ekf.PredictCovariance;

  point_ekf.NowState = point_ekf.UpdateState;
  point_ekf.NowConvariance = point_ekf.UpdateCovariance;

  // ekf.BaseState=ekf.UpdateState;
  // ekf.BaseCovariance=ekf.UpdateCovariance;
}

void CKalman::SetObserve(EKF_PointPara& point_ekf, CMatrix z) {
  // PrintValue(z);
  int Rows = z.m_nNumRows;
  for (int i = 1; i <= Rows; i++) {
    point_ekf.Z.SetElement(i - 1, 0, z.GetElement(i - 1, 0));  //观测向量
  }
}

CMatrix CKalman::SetValue4Cub(double x11, double x12, double x21, double x22) {
  CMatrix matrix_data;
  matrix_data.Init(2, 2);
  matrix_data.Zero();
  double data[4];
  data[0] = x11;
  data[1] = x12;
  data[2] = x21;
  data[3] = x22;
  matrix_data.SetData(data);
  return matrix_data;
}

//对矩阵进行赋值
CMatrix CKalman::SetValue2T(double x11, double x21) {
  CMatrix matrix_data;
  matrix_data.Init(2, 1);
  matrix_data.SetElement(1 - 1, 1 - 1, x11);
  matrix_data.SetElement(2 - 1, 1 - 1, x21);
  return matrix_data;
}

double CKalman::DoubleLineAngle(cv::Point2f line1_p1, cv::Point2f line1_p2,
                                cv::Point2f line2_p1, cv::Point2f line2_p2) {
  double angle_line1, angle_line2;
  double t = abs(line1_p2.y - line1_p1.y);
  if (t < 1) t = 1e-10;
  angle_line1 = atan((line1_p2.x - line1_p1.x) / t);
  t = abs(line2_p2.y - line2_p1.y);
  if (t < 1) t = 1e-10;
  angle_line2 = atan((line2_p2.x - line2_p1.x) / t);
  return (angle_line1 - angle_line2);
}

//由一条直线的两点，两线的夹角，另一个直线的点，求另一条直线给定y值的点坐标
double CKalman::FindLinePara(cv::Point2f p1, cv::Point2f p2, double angle,
                             cv::Point2f p3, double y) {
  double angle_line1 = atan((p2.x - p1.x) / (p2.y - p1.y));
  double angle_line2 = angle_line1 - angle;
  double k = tan(angle_line2);
  double b = p3.x - k * p3.y;
  double x = k * y + b;
  return x;
}

void CKalman::ExcludeOutKalmanLinesByCurrent(
    std::vector<LaneLine>& tempResPriSaveLine) {
  int nSize = tempResPriSaveLine.size();
  if (nSize < 2) return;

  int nLeftIndex = -1;
  int nRightIndex = -1;
  int nLeftValue = -1;
  int nRightValue = -1;

  for (int i = 0; i < nSize; i++) {
    if (tempResPriSaveLine[i].nIsCurLane == 1) {
      nLeftIndex = tempResPriSaveLine[i].nIndex;
      nLeftValue = tempResPriSaveLine[i].near_field[2].x;
    }
    if (tempResPriSaveLine[i].nIsCurLane == 2) {
      nRightIndex = tempResPriSaveLine[i].nIndex;
      nRightValue = tempResPriSaveLine[i].near_field[2].x;
    }
  }

  std::vector<short> select_remote;
  if (nLeftIndex != -1 && nRightIndex != -1) {
    int left_center = -1;
    int left_bottom = -1;
    int right_center = -1;
    int right_bottom = -1;
    //寻找kalman滤波器中对应的中场截距
    for (int j = 0; j < kNumEkf; j++) {
      if (line_ekf_[j].Line.nIndex == nLeftIndex) {
        left_center = line_ekf_[j].point_middle.NowState.GetElement(0, 0);
        left_bottom = line_ekf_[j].point_bottom.NowState.GetElement(0, 0);
      }
      if (line_ekf_[j].Line.nIndex == nRightIndex) {
        right_center = line_ekf_[j].point_middle.NowState.GetElement(0, 0);
        right_bottom = line_ekf_[j].point_bottom.NowState.GetElement(0, 0);
      }
    }

    //将位于左右两线的线失能
    for (int j = 0; j < kNumEkf; j++) {
      int now_center = line_ekf_[j].point_middle.NowState.GetElement(0, 0);
      int now_bottom = line_ekf_[j].point_bottom.NowState.GetElement(0, 0);
      if ((now_center - left_center) < lane_width / 3 ||
          (right_center - now_center) > lane_width / 3) {
        line_ekf_[j].point_middle.time_continu_obsev = 0;
        line_ekf_[j].point_bottom.time_continu_obsev = 0;
        line_ekf_[j].point_top.time_continu_obsev = 0;

        // cout << "flag:" << EKF_Near_Middle[j].flag_work << " index=" <<
        // EKF_Near_Middle[j].Line.nIndex << endl;
      } else if ((now_center > left_center && now_bottom < left_bottom) ||
                 (now_center < left_center && now_bottom > left_bottom) ||
                 (now_center < right_center && now_bottom > right_bottom) ||
                 (now_center > right_center && now_bottom < right_bottom)) {
        line_ekf_[j].point_middle.time_continu_obsev = 0;
        line_ekf_[j].point_bottom.time_continu_obsev = 0;
        line_ekf_[j].point_top.time_continu_obsev = 0;
      }
    }
  }

  //  fittingKalmanResult();
  //  g_vecResPriSaveLine.clear();
  //  g_vecResPriSaveLine = CKalman::Instance()->push_EKF_Result();
}
}
